#include "mapping/map_saver.h"

Map_saver::Map_saver(std::string yaml_path_,std::string map_data_path_){
    yaml_path = yaml_path_;
    map_data_path = map_data_path_;
}
void Map_saver::save_map(const nav_msgs::OccupancyGrid& grid_map){
    map_width   = grid_map.info.width;
    map_height  = grid_map.info.height;
    map_res     = grid_map.info.resolution;
    map_ori_x   = grid_map.info.origin.position.x;
    map_ori_y   = grid_map.info.origin.position.y;
    map_ori_z   = grid_map.info.origin.position.z;
    map_ori_q_w = grid_map.info.origin.orientation.w;
    map_ori_q_x = grid_map.info.origin.orientation.x;
    map_ori_q_y = grid_map.info.origin.orientation.y;
    map_ori_q_z = grid_map.info.origin.orientation.z;
    map_frame_id= grid_map.header.frame_id;
    //! in fact map_ori_q_x/y/z/w not used 

    std::fstream of_map_yaml,of_map_data;
    of_map_yaml.open(yaml_path, std::ios::out);
    if(!of_map_yaml.is_open()){
        LOG(WARNING) << "error opening yaml_file " << yaml_path;
        return ;
    }
    of_map_yaml << "map_width: "    << map_width  << std::endl
                << "map_height: "   << map_height << std::endl
                << "map_res: "      << map_res    << std::endl
                << "map_ori_x: "    << map_ori_x  << std::endl
                << "map_ori_y: "    << map_ori_y  << std::endl
                << "map_ori_z: "    << map_ori_z  << std::endl
                << "map_frame_id: " << map_frame_id;
    of_map_yaml.close();

    of_map_data.open(map_data_path, std::ios::out);
    if(!of_map_data.is_open()){
        LOG(WARNING) << "error opening map_data_file " << map_data_path;
        return ;
    }    
    for(unsigned int i = 0;i < map_height;i++){
        for(unsigned int j = 0;j < map_width;j++){
            int data = grid_map.data[i*map_width + j];
            if(data == 0 || data == -1 || data == 100){
                of_map_data << data << " ";
            }
            else{
                LOG(ERROR) << "invalid data int grid_map.index: " <<  i*map_width + j << "    val = " << data;
                ros::shutdown();
            }
        }
        of_map_data << std::endl;
    }
    of_map_data.close();
}